/*
 *  jarvismarch.cpp
 *  bgsubtraction
 *
 *  Created by a1gucis on 5/18/09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "jarvismarch.h"
JarvisMarch::JarvisMarch() {
	
}

int JarvisMarch::eucDistSquared(int row1, int col1, int row2, int col2) {
	int dist1 = (row2-row1)*(row2-row1);
	int dist2 = (col2-col1)*(col2-col1);
	return dist1+dist2;
}

void JarvisMarch::findFarthestPoints(vector<int> *rowVector, vector<int> *columnVector, int &row1, int &col1, int &row2, int &col2) {
	int maxDist = 0;
	for (int i=0;i<rowVector->size();i++) {
		int r1 = (*rowVector)[i];
		int c1 = (*columnVector)[i];
		for (int j=i+1;j<rowVector->size();j++) {
			int r2 = (*rowVector)[j];
			int c2 = (*columnVector)[j];
			int dist = eucDistSquared(r1, c1, r2, c2);
			if (dist > maxDist) {
				maxDist = dist;
				row1 = r1;
				col1 = c1;
				row2 = r2;
				col2 = c2;
			}
			
		}
	}
}

bool JarvisMarch::findHullPoint(vector<int> *rowVector, vector<int> *columnVector, bool right, int mainRow, int mainCol, int &nextRow, int &nextCol) {
	cout<<"mainRow: "<<mainRow<<", mainCol: "<<mainCol<<endl;;
	bool hullPointFound = false;
	int firstQRow, firstQCol, secondQRow, secondQCol;
	float minSinAngleFirstQuarter = 2.0;
	float minSinAngleSecondQuarter = 0.0;
	float maxVectLengthFirstQuarter = 0.0;
	float maxVectLengthSecondQuarter = 0.0;
	int mainVectRow = 0;
	int mainVectCol = 1;
	if (!right)
		mainVectCol = -1;
	int mainNormalRow = mainVectCol;
	int mainNormalCol = mainVectRow;
	bool firstQuarter = false;
	for (int i=0;i<rowVector->size();i++) {
		int row = (*rowVector)[i];
		int col = (*columnVector)[i];
		if ((row == mainRow) && (col == mainCol))
			continue;
		int vectRow = row - mainRow;
		int vectCol = col - mainCol;
		int mainCrossProduct = mainVectCol*vectRow - vectCol*mainVectRow;
		int normCrossProduct = mainNormalCol*vectRow - vectCol*mainNormalRow;
		if ((mainCrossProduct >= 0) && (normCrossProduct <= 0)) { //vector is in the first quarter
			firstQuarter = true;
			float vectLength = sqrt(vectRow*vectRow+vectCol*vectCol);
			float sinAngle = 2.0;
			if (vectLength > 0)
				sinAngle = (float)mainCrossProduct / vectLength;
			if (sinAngle < minSinAngleFirstQuarter) {
				firstQRow = row;
				firstQCol = col;
				hullPointFound = true;
				minSinAngleFirstQuarter = sinAngle;
			} else if (mainCrossProduct == 0.0) {
				if (vectLength > maxVectLengthFirstQuarter) {
					maxVectLengthFirstQuarter = vectLength;
					firstQRow = row;
					firstQCol = col;
					hullPointFound = true;					
				}
			}
		}
		if ((mainCrossProduct >= 0) && (normCrossProduct > 0) && !firstQuarter) { //vector is in the second quarter
			float vectLength = sqrt(vectRow*vectRow+vectCol*vectCol);
			float sinAngle = 2.0;
			if (vectLength > 0)
				sinAngle = (float)mainCrossProduct / vectLength;
			if (sinAngle > minSinAngleSecondQuarter) {
				secondQRow = row;
				secondQCol = col;
				minSinAngleSecondQuarter = sinAngle;
				hullPointFound = true;
			} else if (mainCrossProduct == 0) {
				if (vectLength > maxVectLengthSecondQuarter) {
					maxVectLengthSecondQuarter = vectLength;
					secondQRow = row;
					secondQCol = col;
					hullPointFound = true;
				}
				
			}
		}
	}
	if (hullPointFound) {
		if (firstQuarter) {
			nextRow = firstQRow;
			nextCol = firstQCol;
		} else {
			nextRow = secondQRow;
			nextCol = secondQCol;
		}
	}
	return hullPointFound;
}

void JarvisMarch::findConvexHull(vector<int> *rowVector, vector<int> *columnVector, vector<ContourPixel *> *hullPixels, int numberOfPixels) {
	int maxRow1, maxCol1, maxRow2, maxCol2;
	findFarthestPoints(rowVector, columnVector, maxRow1, maxCol1, maxRow2, maxCol2);
	ContourPixel *contPixel = new ContourPixel(maxRow1, maxCol1);
	hullPixels->push_back(contPixel);
	contPixel = new ContourPixel(maxRow2, maxCol2);
	hullPixels->push_back(contPixel);
	int prevRow = maxRow1;
	int prevCol = maxCol1;
	//int prevRow = 100;
	//int prevCol = 5;
	int hullRow, hullCol;
	bool hullPointFound = true;
	for (int i=0;i<numberOfPixels && hullPointFound;i++) {
		hullPointFound = findHullPoint(rowVector, columnVector, true, prevRow, prevCol, hullRow, hullCol);
		if (hullPointFound) {
			bool contPixelAdded = false;
			for (int j=0;j<hullPixels->size() && !contPixelAdded;j++) {
				ContourPixel *addedContPixel = (*hullPixels)[j];
				if ((addedContPixel->getRow() == hullRow) && (addedContPixel->getCol() == hullCol))
					contPixelAdded = true;
			}
			if (!contPixelAdded) {
				contPixel = new ContourPixel(hullRow, hullCol);
				hullPixels->push_back(contPixel);
				prevRow = hullRow;
				prevCol = hullCol;
			} else
				hullPointFound = false;
			
		}
	}
	
	prevRow = maxRow2;
	prevCol = maxCol2;
	hullPointFound = true;
	for (int i=0;i<numberOfPixels && hullPointFound;i++) {
		hullPointFound = findHullPoint(rowVector, columnVector, false, prevRow, prevCol, hullRow, hullCol);
		if (hullPointFound) {
			bool contPixelAdded = false;
			for (int j=0;j<hullPixels->size() && !contPixelAdded;j++) {
				ContourPixel *addedContPixel = (*hullPixels)[j];
				if ((addedContPixel->getRow() == hullRow) && (addedContPixel->getCol() == hullCol))
					contPixelAdded = true;
			}
			if (!contPixelAdded) {
				contPixel = new ContourPixel(hullRow, hullCol);
				hullPixels->push_back(contPixel);
				prevRow = hullRow;
				prevCol = hullCol;
			} else
				hullPointFound = false;
		}
	}
	
}